
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_rtl.c
  * @author     baiyang
  * @date       2022-3-10
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"

#include <mavproxy/mavproxy.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void mrtl_climb_start(mode_base_t mode);
static void mrtl_return_start(mode_base_t mode);
static void mrtl_climb_return_run(mode_base_t mode);
static void mrtl_loiterathome_start(mode_base_t mode);
static void mrtl_loiterathome_run(mode_base_t mode);
static void mrtl_build_path(mode_base_t mode);
static void mrtl_compute_return_target(mode_base_t mode);
static void mrtl_pva_control_start(mode_base_t mode, int16_t speed);

static bool mrtl_init(mode_base_t mode, bool ignore_checks);
static void mrtl_run(mode_base_t mode);
static bool mrtl_use_pilot_yaw(mode_base_t mode);
static bool mrtl_get_wp(mode_base_t mode, Location* destination);
static bool mrtl_is_landing(mode_base_t mode);
static uint32_t mrtl_wp_distance(mode_base_t mode);
static int32_t mrtl_wp_bearing(mode_base_t mode);
static ModeNumber mrtl_mode_number(mode_base_t mode);
static bool mrtl_requires_GPS(mode_base_t mode);
static bool mrtl_has_manual_throttle(mode_base_t mode);
static bool mrtl_allows_arming(mode_base_t mode, ArmingMethod method);
static bool mrtl_is_autopilot(mode_base_t mode);
static bool mrtl_requires_terrain_failsafe(mode_base_t mode);
static const char *mrtl_name(mode_base_t mode);
static const char *mrtl_name4(mode_base_t mode);
static float mrtl_crosstrack_error(mode_base_t mode);
/*----------------------------------variable----------------------------------*/
static struct mode_ops mode_rtl_ops = {
        .mode_number = mrtl_mode_number,
        .init        = mrtl_init,
        .exit        = NULL,
        .run         = mrtl_run,
        .requires_GPS = mrtl_requires_GPS,
        .has_manual_throttle = mrtl_has_manual_throttle,
        .allows_arming = mrtl_allows_arming,
        .is_autopilot = mrtl_is_autopilot,
        .has_user_takeoff = NULL,
        .in_guided_mode = NULL,
        .logs_attitude = NULL,
        .allows_save_trim = NULL,
        .allows_autotune = NULL,
        .allows_flip = NULL,
        .name = mrtl_name,
        .name4 = mrtl_name4,
        .is_taking_off = NULL,
        .is_landing = mrtl_is_landing,
        .requires_terrain_failsafe = mrtl_requires_terrain_failsafe,
        .get_wp = mrtl_get_wp,
        .wp_bearing = mrtl_wp_bearing,
        .wp_distance = mrtl_wp_distance,
        .crosstrack_error = mrtl_crosstrack_error,
        .output_to_motors = NULL,
        .use_pilot_yaw = mrtl_use_pilot_yaw,
        .throttle_hover = NULL,
        .do_user_takeoff_start = NULL,
        .pause = NULL,
        .resume = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   mode_rtl  
  * @param[out]  
  * @retval      
  * @note        
  */
void mode_rtl_ctor(ModeRTL* mode_rtl)
{
    mode_ctor(&mode_rtl->mode, &mode_rtl_ops);

    mode_rtl->_state = RTL_INITIAL_CLIMB;
    mode_rtl->_state_complete = false;
}

/*
 * Init and run calls for RTL flight mode
 *
 * There are two parts to RTL, the high level decision making which controls which state we are in
 * and the lower implementation of the waypoint or landing controllers within those states
 */

// rtl_init - initialise rtl controller
static bool mrtl_init(mode_base_t mode, bool ignore_checks)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    if (!ignore_checks) {
        if (!fms.ahrs->home_is_set) {
            return false;
        }
    }

    // initialise waypoint and spline controller
    mrtl_pva_control_start(mode, fms.g.rtl_speed_cms);
    mode_rtl->_state = RTL_STARTING;
    mode_rtl->_state_complete = true; // see run() method below
    mode_rtl->terrain_following_allowed = !fms.failsafe.terrain;

    // reset flag indicating if pilot has applied roll or pitch inputs during landing
    fms.ap.land_repo_active = false;

    return true;
}

static void mrtl_run(mode_base_t mode)
{
    return mrtl_run2(mode, true);
}

// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
void mrtl_run2(mode_base_t mode, bool disarm_on_land)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    if (!mode->motors->_armed) {
        return;
    }

    // check if we need to move to next state
    if (mode_rtl->_state_complete) {
        switch (mode_rtl->_state) {
        case RTL_STARTING:
            mrtl_build_path(mode);
            mrtl_climb_start(mode);
            break;
        case RTL_INITIAL_CLIMB:
            mrtl_return_start(mode);
            break;
        case RTL_RETURN_HOME:
            mrtl_loiterathome_start(mode);
            break;
        case RTL_LOITER_AT_HOME:
            if (mode_rtl->rtl_path.land || fms.failsafe.radio) {
                mrtl_land_start(mode);
            }else{
                mrtl_descent_start(mode);
            }
            break;
        case RTL_FINAL_DESCENT:
            // do nothing
            break;
        case RTL_LAND:
            // do nothing - rtl_land_run will take care of disarming motors
            break;
        }
    }

    // call the correct run function
    switch (mode_rtl->_state) {

    case RTL_STARTING:
        // should not be reached:
        mode_rtl->_state = RTL_INITIAL_CLIMB;

    case RTL_INITIAL_CLIMB:
        mrtl_climb_return_run(mode);
        break;

    case RTL_RETURN_HOME:
        mrtl_climb_return_run(mode);
        break;

    case RTL_LOITER_AT_HOME:
        mrtl_loiterathome_run(mode);
        break;

    case RTL_FINAL_DESCENT:
        mrtl_descent_run(mode);
        break;

    case RTL_LAND:
        mrtl_land_run(mode, disarm_on_land);
        break;
    }
}

// rtl_descent_start - initialise descent to final alt
void mrtl_descent_start(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    mode_rtl->_state = RTL_FINAL_DESCENT;
    mode_rtl->_state_complete = false;

    // Set wp navigation target to above home
    loiter_init_target3(mode->loiter_nav, &mode_rtl->rtl_path.destination_neu);

    // initialise altitude target to stopping point
    posctrl_init_z_controller_stopping_point(mode->pos_control);

    // initialise yaw
    autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
}

// rtl_descent_run - implements the final descent to the RTL_ALT
//      called by rtl_run at 100hz or more
void mrtl_descent_run(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    float target_roll = 0.0f;
    float target_pitch = 0.0f;
    float target_yaw_rate = 0.0f;

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // process pilot's input
    if (!fms.failsafe.radio) {
        if ((fms.g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && lpf_get_output(&fms.rc_throttle_control_in_filter) > LAND_CANCEL_TRIGGER_THR){
            //AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
            // exit land if throttle is high
            if (!fms_set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
                fms_set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
            }
        }

        if (fms.g.land_repositioning) {
            // apply SIMPLE mode transform to pilot inputs
            //update_simple_mode();

            // convert pilot input to lean angles
            fms_get_pilot_desired_lean_angles(&target_roll, &target_pitch, loiter_get_angle_max_cd(mode->loiter_nav), attctrl_get_althold_lean_angle_max_cd(mode->attitude_control));

            // record if pilot has overridden roll or pitch
            if (!math_flt_zero(target_roll) || !math_flt_zero(target_pitch)) {
                if (!fms.ap.land_repo_active) {
                    //AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE);
                }
                fms.ap.land_repo_active = true;
            }
        }

        if (fms.g.land_repositioning || mrtl_use_pilot_yaw(mode)) {
            // get pilot's desired yaw rate
            target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(fms.channel_yaw));
        }
    }

    // set motors to full range
    Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // process roll, pitch inputs
    loiter_set_pilot_desired_acceleration(mode->loiter_nav, target_roll, target_pitch);

    // run loiter controller
    loiter_update(mode->loiter_nav, true);

    // WP_Nav has set the vertical position control targets
    // run the vertical position controller and set output throttle
    posctrl_set_alt_target_with_slew(mode->pos_control, mode_rtl->rtl_path.descent_target.alt);
    posctrl_update_z_controller(mode->pos_control);

    Vector3f_t thrust_vector = loiter_get_thrust_vector(mode->loiter_nav);

    // roll & pitch from waypoint controller, yaw rate from pilot
    attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, target_yaw_rate);

    // check if we've reached within 20cm of final altitude
    mode_rtl->_state_complete = labs(mode_rtl->rtl_path.descent_target.alt - mode->ahrs->curr_loc.alt) < 20;
}

// land_start - initialise controllers to loiter over home
void mrtl_land_start(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    mode_rtl->_state = RTL_LAND;
    mode_rtl->_state_complete = false;

    // Set wp navigation target to above home
    loiter_init_target3(mode->loiter_nav, &mode_rtl->rtl_path.destination_neu);

    // initialise the vertical position controller
    if (!posctrl_is_active_z(mode->pos_control)) {
        posctrl_init_z_controller(mode->pos_control);
    }

    // initialise yaw
    autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
}

// land_run - run the landing controllers to put the aircraft on the ground
// called by rtl_run at 100hz or more
void mrtl_land_run(mode_base_t mode, bool disarm_on_land)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    // check if we've completed this stage of RTL
    mode_rtl->_state_complete = fms.ap.land_complete;

    // disarm when the landing detector says we've landed
    if (disarm_on_land && fms.ap.land_complete && mode->motors->_spool_state == MOTOR_GROUND_IDLE) {
        arming_disarm(mode->arming, ARMING_CHECK_LANDED, true);
    }

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        mode_make_safe_ground_handling(mode, false);
        loiter_clear_pilot_desired_acceleration(mode->loiter_nav);
        loiter_init_target(mode->loiter_nav);
        return;
    }

    // set motors to full range
    Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // run normal landing or precision landing (if enabled)
    mode_land_run_normal_or_precland(mode, false);
}

enum RTLAltType mrtl_get_alt_type()
{
    // sanity check parameter
    if (fms.g.rtl_alt_type < 0 || fms.g.rtl_alt_type > (int)RTL_ALTTYPE_TERRAIN) {
        return RTL_ALTTYPE_RELATIVE;
    }
    return (enum RTLAltType)fms.g.rtl_alt_type;
}

// re-start RTL with terrain following disabled
void mrtl_restart_without_terrain(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    //AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL);
    mode_rtl->terrain_following_allowed = false;
    mode_rtl->_state = RTL_STARTING;
    mode_rtl->_state_complete = true;
    mavproxy_send_statustext(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing");
}



/// reached_destination - true when we have come within RADIUS cm of the waypoint
bool mrtl_reached_wp_destination(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    Vector3f_t curr_destination_diff;
    vec3_sub(&curr_destination_diff, &mode_rtl->rtl_path.destination_neu, &mode->ahrs->relpos_cm);

    return vec3_length_squared(&curr_destination_diff) < sq(wpnav_get_wp_radius_cm(mode->wp_nav));
}

// rtl_climb_start - initialise climb to RTL altitude
static void mrtl_climb_start(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    mode_rtl->_state = RTL_INITIAL_CLIMB;
    mode_rtl->_state_complete = false;

    // set the destination
    if (!location_get_vector_from_home_neu(&mode_rtl->rtl_path.climb_target, &mode_rtl->rtl_path.destination_neu)
        || !location_get_vector_from_home_neu(&mode_rtl->rtl_path.return_target, &mode_rtl->rtl_path.destination_neu)) {
        // this should not happen because rtl_build_path will have checked terrain data was available
        mavproxy_send_statustext(MAV_SEVERITY_CRITICAL,"RTL: unexpected error setting climb target");
        //AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
        fms_set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
        return;
    } else {
        posctrl_input_pos_xyz(mode->pos_control, &mode_rtl->rtl_path.destination_neu, 0.0f, 0.0f);
    }

    // hold current yaw during initial climb
    autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
}

// rtl_return_start - initialise return to home
static void mrtl_return_start(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    mode_rtl->_state = RTL_RETURN_HOME;
    mode_rtl->_state_complete = false;

    // set the destination
    if (!location_get_vector_from_home_neu(&mode_rtl->rtl_path.return_target, &mode_rtl->rtl_path.destination_neu)) {
        mrtl_restart_without_terrain(mode);
    } else {
        posctrl_input_pos_xyz(mode->pos_control, &mode_rtl->rtl_path.destination_neu, 0.0f, 0.0f);
    }

    // initialise yaw to point home (maybe)
    autoyaw_set_mode_to_default(mode->auto_yaw, true);
}

// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
//      called by rtl_run at 100hz or more
static void mrtl_climb_return_run(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!fms.failsafe.radio && mrtl_use_pilot_yaw(mode)) {
        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(fms.channel_yaw));
        if (!math_flt_zero(target_yaw_rate)) {
            autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
        }
    }

    // set motors to full range
    Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // run waypoint controller
    posctrl_input_pos_xyz(mode->pos_control, &mode_rtl->rtl_path.destination_neu, 0.0f, 0.0f);

    // run position controllers
    posctrl_update_xy_controller(mode->pos_control);
    posctrl_update_z_controller(mode->pos_control);

    Vector3f_t thrust_vector = posctrl_get_thrust_vector(mode->pos_control);

    // call attitude controller
    if (mode->auto_yaw->_mode == AUTO_YAW_HOLD) {
        // roll & pitch from waypoint controller, yaw rate from pilot
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, target_yaw_rate);
    } else {
        // roll, pitch from waypoint controller, yaw heading from auto_heading()
        attctrl_input_thrust_vector_heading(mode->attitude_control, &thrust_vector, autoyaw_yaw(mode->auto_yaw), 0.0f);
    }

    // check if we've completed this stage of RTL
    mode_rtl->_state_complete = mrtl_reached_wp_destination(mode);
}

// loiterathome_start - initialise return to home
static void mrtl_loiterathome_start(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    mode_rtl->_state = RTL_LOITER_AT_HOME;
    mode_rtl->_state_complete = false;
    mode_rtl->_loiter_start_time = time_millis();

    // yaw back to initial take-off heading yaw unless pilot has already overridden yaw
    if (autoyaw_default_mode(true) != AUTO_YAW_HOLD) {
        autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_RESETTOARMEDYAW);
    } else {
        autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
    }
}

// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
//      called by rtl_run at 100hz or more
static void mrtl_loiterathome_run(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!fms.failsafe.radio && mrtl_use_pilot_yaw(mode)) {
        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(fms.channel_yaw));
        if (!math_flt_zero(target_yaw_rate)) {
            autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
        }
    }

    // set motors to full range
    Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // run waypoint controller
    posctrl_input_pos_xyz(mode->pos_control, &mode_rtl->rtl_path.destination_neu, 0.0f, 0.0f);

    // run position controllers
    posctrl_update_xy_controller(mode->pos_control);
    posctrl_update_z_controller(mode->pos_control);

    Vector3f_t thrust_vector = posctrl_get_thrust_vector(mode->pos_control);

    // call attitude controller
    if (mode->auto_yaw->_mode == AUTO_YAW_HOLD) {
        // roll & pitch from waypoint controller, yaw rate from pilot
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, target_yaw_rate);
    } else {
        // roll, pitch from waypoint controller, yaw heading from auto_heading()
        attctrl_input_thrust_vector_heading(mode->attitude_control, &thrust_vector, autoyaw_yaw(mode->auto_yaw), 0.0f);
    }

    // check if we've completed this stage of RTL
    if ((time_millis() - mode_rtl->_loiter_start_time) >= (uint32_t)fms.g.rtl_loiter_time) {
        if (mode->auto_yaw->_mode == AUTO_YAW_RESETTOARMEDYAW) {
            // check if heading is within 2 degrees of heading when vehicle was armed
            if (abs((int32_t)math_wrap_180_cd(mode->ahrs->yaw_sensor_cd-fms.initial_armed_bearing)) <= 200) {
                mode_rtl->_state_complete = true;
            }
        } else {
            // we have loitered long enough
            mode_rtl->_state_complete = true;
        }
    }
}

static void mrtl_build_path(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    // origin point is our stopping point
    Vector3p stopping_point;
    posctrl_get_stopping_point_xy_cm2(mode->pos_control, &stopping_point);
    posctrl_get_stopping_point_z_cm(mode->pos_control, &stopping_point.z);
    Location_from_vector_xy_alt(&mode_rtl->rtl_path.origin_point, &stopping_point, ALT_FRAME_ABOVE_ORIGIN);
    location_change_alt_frame(&mode_rtl->rtl_path.origin_point, ALT_FRAME_ABOVE_HOME);

    // compute return target
    mrtl_compute_return_target(mode);

    // climb target is above our origin point at the return altitude
    location_from_lat_lon_alt(&mode_rtl->rtl_path.climb_target, mode_rtl->rtl_path.origin_point.lat, mode_rtl->rtl_path.origin_point.lng, mode_rtl->rtl_path.return_target.alt, location_get_alt_frame(&mode_rtl->rtl_path.return_target));

    // descent target is below return target at rtl_alt_final
    location_from_lat_lon_alt(&mode_rtl->rtl_path.descent_target, mode_rtl->rtl_path.return_target.lat, mode_rtl->rtl_path.return_target.lng, fms.g.rtl_alt_final, ALT_FRAME_ABOVE_HOME);

    // set land flag
    mode_rtl->rtl_path.land = fms.g.rtl_alt_final <= 0;
}

// compute the return target - home or rally point
//   return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
static void mrtl_compute_return_target(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    // set return target to nearest rally point or home position (Note: alt is absolute)
    ahrs_get_home(fms.ahrs, &mode_rtl->rtl_path.return_target);

    // curr_alt is current altitude above home or above terrain depending upon use_terrain
    int32_t curr_alt = fms.ahrs->curr_loc.alt;

    // determine altitude type of return journey (alt-above-home, alt-above-terrain using range finder or alt-above-terrain using terrain database)
    enum ReturnTargetAltType alt_type = RTL_RELATIVE;
    if (mode_rtl->terrain_following_allowed && (mrtl_get_alt_type() == RTL_ALTTYPE_TERRAIN)) {
        // convert RTL_ALT_TYPE and WPNAV_RFNG_USE parameters to ReturnTargetAltType
        switch (wpnav_get_terrain_source(mode->wp_nav)) {
        case WPNAV_TERRAIN_UNAVAILABLE:
            alt_type = RTL_RELATIVE;
            //AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND);
            mavproxy_send_statustext(MAV_SEVERITY_CRITICAL, "RTL: no terrain data, using alt-above-home");
            break;
        case WPNAV_TERRAIN_FROM_RANGEFINDER:
            alt_type = RTL_RANGEFINDER;
            break;
        case WPNAV_TERRAIN_FROM_TERRAINDATABASE:
            alt_type = RTL_TERRAINDATABASE;
            break;
        }
    }

    // set curr_alt and return_target.alt from terrain database
    if (alt_type == RTL_TERRAINDATABASE) {
        // set curr_alt to current altitude above terrain
        // convert return_target.alt from an abs (above MSL) to altitude above terrain
        //   Note: the return_target may be a rally point with the alt set above the terrain alt (like the top of a building)
        int32_t curr_terr_alt;
        if (location_get_alt_cm(&mode->ahrs->curr_loc, ALT_FRAME_ABOVE_TERRAIN, &curr_terr_alt)&&
            location_change_alt_frame(&mode_rtl->rtl_path.return_target, ALT_FRAME_ABOVE_TERRAIN)) {
            curr_alt = curr_terr_alt;
        } else {
            // fallback to relative alt and warn user
            alt_type = RTL_RELATIVE;
            //AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
            mavproxy_send_statustext(MAV_SEVERITY_CRITICAL, "RTL: no terrain data, using alt-above-home");
        }
    }

    // for the default case we must convert return-target alt (which is an absolute alt) to alt-above-home
    if (alt_type == RTL_RELATIVE) {
        if (!location_change_alt_frame(&mode_rtl->rtl_path.return_target, ALT_FRAME_ABOVE_HOME)) {
            // this should never happen but just in case
            location_set_alt_cm(&mode_rtl->rtl_path.return_target, 0, ALT_FRAME_ABOVE_HOME);
            mavproxy_send_statustext(MAV_SEVERITY_WARNING, "RTL: unexpected error calculating target alt");
        }
    }

    // set new target altitude to return target altitude
    // Note: this is alt-above-home or terrain-alt depending upon rtl_alt_type
    // Note: ignore negative altitudes which could happen if user enters negative altitude for rally point or terrain is higher at rally point compared to home
    int32_t target_alt = MAX(mode_rtl->rtl_path.return_target.alt, 0);

    // increase target to maximum of current altitude + climb_min and rtl altitude
    target_alt = MAX(target_alt, curr_alt + MAX(0, fms.g.rtl_climb_min));
    target_alt = MAX(target_alt, MAX(fms.g.rtl_altitude, RTL_ALT_MIN));

    // reduce climb if close to return target
    float rtl_return_dist_cm = location_get_distance(&mode_rtl->rtl_path.return_target, &mode_rtl->rtl_path.origin_point) * 100.0f;
    // don't allow really shallow slopes
    if (fms.g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) {
        target_alt = MAX(curr_alt, MIN(target_alt, MAX(rtl_return_dist_cm*fms.g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB)));
    }

    // set returned target alt to new target_alt (don't change altitude type)
    location_set_alt_cm(&mode_rtl->rtl_path.return_target, target_alt, (alt_type == RTL_RELATIVE) ? ALT_FRAME_ABOVE_HOME : ALT_FRAME_ABOVE_TERRAIN);

    // ensure we do not descend
    mode_rtl->rtl_path.return_target.alt = MAX(mode_rtl->rtl_path.return_target.alt, curr_alt);
}

// initialise position controller
static void mrtl_pva_control_start(mode_base_t mode, int16_t speed)
{
    // initialise horizontal speed, acceleration
    posctrl_set_max_speed_accel_xy(mode->pos_control, speed, wpnav_get_wp_acceleration(mode->wp_nav));
    posctrl_set_correction_speed_accel_xy(mode->pos_control, speed, wpnav_get_wp_acceleration(mode->wp_nav));

    // initialize vertical speeds and acceleration
    posctrl_set_max_speed_accel_z(mode->pos_control, wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav), wpnav_get_accel_z(mode->wp_nav));
    posctrl_set_correction_speed_accel_z(mode->pos_control, wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav), wpnav_get_accel_z(mode->wp_nav));

    // initialise velocity controller
    posctrl_init_z_controller(mode->pos_control);
    posctrl_init_xy_controller(mode->pos_control);

    // initialise yaw
    autoyaw_set_mode_to_default(mode->auto_yaw, true);
}

///
// returns true if pilot's yaw input should be used to adjust vehicle's heading
static bool mrtl_use_pilot_yaw(mode_base_t mode)
{
    return (fms.g.rtl_options & (uint32_t)RtlIgnorePilotYaw) == 0;
}

static bool mrtl_get_wp(mode_base_t mode, Location* destination)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    // provide target in states which use wp_nav
    switch (mode_rtl->_state) {
    case RTL_STARTING:
    case RTL_INITIAL_CLIMB:
    case RTL_RETURN_HOME:
    case RTL_LOITER_AT_HOME:
    case RTL_FINAL_DESCENT: {
        Location_from_vector_xy_alt(destination, &mode_rtl->rtl_path.destination_neu, ALT_FRAME_ABOVE_HOME);
        return mode->ahrs->valid_lpos;
    }
    case RTL_LAND:
        return false;
    }

    // we should never get here but just in case
    return false;
}

static bool mrtl_is_landing(mode_base_t mode)
{
    return ((ModeRTL*)mode)->_state == RTL_LAND;
}

static uint32_t mrtl_wp_distance(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    Vector3f_t curr_destination_diff;
    vec3_sub(&curr_destination_diff, &mode_rtl->rtl_path.destination_neu, &mode->ahrs->relpos_cm);

    return vec3_length(&curr_destination_diff);
}

static int32_t mrtl_wp_bearing(mode_base_t mode)
{
    ModeRTL* mode_rtl = (ModeRTL*)mode;

    return vec3_get_bearing_cd(&mode->ahrs->relpos_cm, &mode_rtl->rtl_path.destination_neu);
}

static ModeNumber mrtl_mode_number(mode_base_t mode) { return RTL; }
static bool mrtl_requires_GPS(mode_base_t mode) { return true; }
static bool mrtl_has_manual_throttle(mode_base_t mode) { return false; }
static bool mrtl_allows_arming(mode_base_t mode, ArmingMethod method) { return false; };
static bool mrtl_is_autopilot(mode_base_t mode) { return true; }
static bool mrtl_requires_terrain_failsafe(mode_base_t mode) { return true; }
static const char *mrtl_name(mode_base_t mode) { return "RTL"; }
static const char *mrtl_name4(mode_base_t mode) { return "RTL "; }
static float mrtl_crosstrack_error(mode_base_t mode) { return posctrl_crosstrack_error(mode->pos_control);}
/*------------------------------------test------------------------------------*/


